
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include <ti/driverlib/m0p/dl_interrupt.h>
#include "motion.h"
#include "line_patrol.h"
#include "servo.h"
#include "algorithm.h"
#include "basic.h"

#define TAG "Main"
 char __str_all[128] ;
// PI控制器结构体 (使用float)

int _main()
{
  // 测试参数
  const float target_radius = 6.0f; // 目标转弯半径(米)
  float actual_radius = 10.0f;      // 初始实际转弯半径(米)
  const float dt = 0.1f;            // 控制周期(秒)
  const float total_time = 10.0f;   // 总模拟时间(秒)
  const int steps = (int)(total_time / dt);

  // 初始化PI控制器
  pi_control_t pi;
  pi_init(&pi, 1.8f, 1.7f, 5.0f); // Kp=0.8, Ki=0.2

  my_printf("PI Controller Test for Turning Radius (float version)\n");
  my_printf("Target Radius: %.2f m\n", target_radius);
  my_printf("Initial Radius: %.2f m\n", actual_radius);
  my_printf("Control Parameters: Kp=%.2f, Ki=%.2f, dt=%.2fs\n\n", pi.Kp, pi.Ki, dt);

  // 模拟控制循环
  for (int i = 0; i < steps; i++)
  {
    float error = target_radius - actual_radius;
    float control = pi_update(&pi, error,dt);

    // 简化系统模型：实际半径变化与控制输出成正比
    actual_radius += control * dt;

    // 每10步打印一次状态
    if (i % 10 == 0)
    {
      my_printf("Time: %.1fs | Radius: %.4fm | Error: %.4fm | Control: %.4f\n",
                i * dt, actual_radius, error, control);
    }
  }

  // 最终结果
  float final_error = target_radius - actual_radius;
  my_printf("\nFinal Results:\n");
  my_printf("Target Radius: %.4f m\n", target_radius);
  my_printf("Actual Radius: %.4f m\n", actual_radius);
  my_printf("Final Error: %.4f m\n", final_error);
  my_printf("Steady-state: %s\n", fabsf(final_error) < 0.01f ? "Yes" : "No");

  return 0;
}

int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();

  uart_init();

  log(TAG, "Init Done!");

  _main();
  uint32_t millis = 0;
  while (1)
  {
    mspm0_delay_ms(1000);
    mspm0_get_clock_ms(&millis);
    log("Time", "%ld", millis);
  }
}
